package ext.sim;

import context.render.vertexdata.VertexData;
import ext.sim.contact.detect.TreeSphere;
import java.util.Collection;
import java.util.HashSet;
import java.util.LinkedList;
import math.mat3;
import math.region.Sphere3;
import math.vec3;
import math.vec4;

public abstract class Body
{
    
    // ----------- COLLISION STRUCTURES ------------ //
    /** Internal heirarchy of spheres for collision detection. NOTE: SUBCLASSES MUST INITIALIZE THIS! */
    protected TreeSphere mTreeSphere;
    
    
    // ----------- CONSTANT QUANTATIES ------------ // 
    /** Pinned constraint. */
    public boolean mIsPinned;
    
    /** Mass. */
    public double mMass;
    
    /** Inertia tensor in body-space. */
    public mat3 mIbody;
    
    /** Inverse of inertia tensor in body-space. */
    public mat3 mInvIbody;
    
    
    
    // ----------- STATE VARIABLES ------------ // 
    /** Position. Defined:
        d/dt[X] = V         */
    public vec3 mX;
    
    /** Rotation stored in a quaternion. Defined:
        d/dt[Q] = 1/2 * (0,W) * Q, where (0,W) is the quaternion with real value 0, imaginary value W.      */
    public vec4 mQ;
    
    /** Momentum. Defined:
        d/dt[P] = F         */
    public vec3 mP;
    
    /** Angular momentum. Defined: 
        d/dt[L] = T         */
    public vec3 mL;
    
    
    
    // ----------- DERIVED QUANTITIES ------------ // 
    /** Inertia tensor at time t. Defined:
        I = R * Ibody * Transpose(R)              */
    public mat3 mI;
    
    /** Inverse of inertia tensor at time t. Defined: 
        InvI = R * Inverse(Ibody) * Transpose(R)            */
    public mat3 mInvI;
    
    /** Rotation matrix at time t. Defined: 
        R = QuaternionToMatrix(normalize(Q))
        d/dt[R] = crossMatrix(W) * R            */
    public mat3 mR;
    
    /** Inverse of the rotation matrix. */
    public mat3 mInvR;
    
    /** Linear velocity, in lengths/sec. Defined: 
        V = P / Mass.       */
    public vec3 mV;
    
    /** Angular velocity, in radians/sec. Defined: 
        W = Inverse(I) * L            */
    public vec3 mW;
    
    
    
    // ----------- COMPUTED QUANTITIES ------------ // 
    /** Force.  */
    public vec3 mF;
    
    /** Torque. */
    public vec3 mT;
    
    
    
    public Body()
    {
        this.mTreeSphere = null; // Requires initialization in subclass!
        
        this.mIsPinned = false;
        this.mMass = 1.0;
        
        this.mIbody = new mat3();
        this.mInvIbody = new mat3();
        
        this.mX = new vec3();
        this.mQ = new vec4(vec3.ZERO, 1.0); // Plausible orientation: ( sin(0/2)*(a, b, c), cos(0/2) )
        this.mP = new vec3();
        this.mL = new vec3();
        
        this.mI = new mat3();
        this.mInvI = new mat3();
        this.mR = new mat3();
        this.mInvR = new mat3();
        this.mV = new vec3();
        this.mW = new vec3();
        
        this.mF = new vec3();
        this.mT = new vec3();
    }
    
    
    public final void transformInternalsToW()
    {
        this.mTreeSphere.transformToWorldSpace(this.mR, this.mX);
    }
    
    public final void transformInternalsToB()
    {
        this.mTreeSphere.transformToBodySpace();
    }
    
    
    /** Computes two sets that form candidate Block objects for contact. Body I and J assumed to be in world-space internally. */
    public final void overlapCandidates(
            Body other, 
            Collection<Sphere3> candidI, 
            Collection<Sphere3> candidJ)
    {
        LinkedList<TreeSphere.Node[]> nodePairCandid = this.mTreeSphere.nodePairOverlaps(other.mTreeSphere);
        
        HashSet<Sphere3> setI = new HashSet<>(); // Hashing to avoid duplicates.
        HashSet<Sphere3> setJ = new HashSet<>();
        
        for (TreeSphere.Node[] nodePair : nodePairCandid)
        {
            for (Sphere3 s : nodePair[0].mSpheres)
            {
                if (!setI.contains(s))
                {
                    setI.add(s);
                    candidI.add(s);
                }
            }
            for (Sphere3 s : nodePair[1].mSpheres)
            {
                if (!setJ.contains(s))
                {
                    setJ.add(s);
                    candidJ.add(s);
                }
            }
        }
    }
    
    
    public final Sphere3 getBoundingSphere()
    {
        return this.mTreeSphere.mRoot.mBounds;
    }
    
    
    public abstract void render(VertexData aVertexData);
    
    
    public final void renderDebug(VertexData aVD)
    {
        this.mTreeSphere.render(aVD);
    }
    
}
